#include "ePuck.h"
#include <stdio.h>
#include <pthread.h>
#include <termios.h> 
#include <unistd.h>
#include <stdlib.h>


int GetChar( )
{
  #define MAGIC_MAX_CHARS 1
  struct termios initial_settings;
  struct termios settings;
  unsigned char  keycodes[ MAGIC_MAX_CHARS ];

  tcgetattr( STDIN_FILENO, &initial_settings );
  settings = initial_settings;

  /* Set the console mode to no-echo, raw input. */
  /* The exact meaning of all this jazz will be discussed later. */
  settings.c_cc[ VTIME ] = 1;
  settings.c_cc[ VMIN  ] = MAGIC_MAX_CHARS;
  settings.c_iflag &= ~(IXOFF);
  settings.c_lflag &= ~(ECHO | ICANON);
  tcsetattr( STDIN_FILENO, TCSANOW, &settings );

  keycodes[0] = getchar();
  //count = read( stdin, (void*)keycodes, MAGIC_MAX_CHARS );

  tcsetattr( STDIN_FILENO, TCSANOW, &initial_settings );

  return keycodes[0];
}


void
PrintResult(  const ePuck::EResult Result, 
              const std::string& rCustom )
{
  if( ePuck::EResultOk != Result )
  {
    printf
    ( 
    " [FAIL] %s failed with code: %d\n" 
    , rCustom.c_str()
    , Result  
    );
  }
  fflush(stdout);
}

int 
main(int argc, char** argv)
{
  ePuck::EResult Result = ePuck::EResultOk;
  int StepSize = 10;
  unsigned Port = 0;
  if( 2 == argc || 3 == argc )
  {
    if( '?' == argv[ 1 ][0] )
    {
      printf
      (
        "%s [StepSize = 10] [Port = 0]\n" \
        "w: forward\n"\
        "s: backwards \n"\
        "a: left\n"\
        "d: right\n"\
        "q: stop and quit\n"\
        "q: stop\n"\
        "e: toggle front-led\n"\
      , argv[ 0 ]
      );  
      return 1;
    }
    else 
    {
      StepSize = atoi( argv[ 1 ] );
      printf( "Using alternative step-size: %d\n", StepSize );
      if( 3 == argc )
      {
        Port = atoi( argv[ 2 ] );
        printf( "Using alternative bluetooth port: %d\n", Port );
      }
    }
  }

  // Create ePuck device
  ePuck::Device ePuck;
  
  // Connect to device
  if( ePuck::EResultOk == Result )
  {
    char ConnectString[] = "/dev/rfcommX";
    sprintf( ConnectString,"/dev/rfcomm%d", Port );
    Result = ePuck.Connect( ConnectString );
    PrintResult( Result, "Connection" );
  }
  
  // Thread receiving key-presses
  // pthread_t KeyThread;

  bool Continue = true;
  int  Left = 0, Right = 0;
  bool LedState = false;
  ePuck::SProximity Proximity;
  while( ePuck::EResultOk == Result && Continue )
  {
    // Temporarily add the proximity to the control program
    Result = ePuck.GetProximity( Proximity );
    printf
    ( 
      "%u %u %u %u %u %u %u %u\n"
    , Proximity.S0
    , Proximity.S1
    , Proximity.S2
    , Proximity.S3
    , Proximity.S4
    , Proximity.S5
    , Proximity.S6
    , Proximity.S7
    );
    //

    char Key = GetChar(); 
    switch( Key )
    {
      case 'w':
      {
        Left  += StepSize;
        Right += StepSize;
        Result = ePuck.Move( Left, Right );
        break;

      }
      case 'a':
      {
        Left -= StepSize;
        Right += StepSize;
        Result = ePuck.Move( Left, Right );
        break;
      }
      case 'd':
      {
        Left += StepSize;
        Right -= StepSize;
        Result = ePuck.Move( Left, Right );
        break;
      }
      case 's':
      {
        Left -= StepSize;
        Right -= StepSize;
        Result = ePuck.Move( Left, Right );
        break;
      }
      case 'q':
      {
        Result = ePuck.Stop( );
        Result = ePuck.Sound( ePuck::ESoundEuh );
        Left = 0;
        Right = 0;
        Continue = false;
        break;
      }
      case 'f':
      {
        Result = ePuck.Stop( );
        Result = ePuck.Sound( ePuck::ESoundBang );
        Left = 0;
        Right = 0;
        break;
      }
      case 'e':
      {
        Result = ePuck.SetLed
        (
          ePuck::ELedIdFront
        , LedState ? ePuck::ELedStateOn : ePuck::ELedStateOff 
        );
        LedState = !LedState;
        break;
      }
      default:
      {
        printf( "Key ( %d ) not supported\n", Key );
      }
    }
    PrintResult( Result, "Command" );
  }
}
